#include <rclcpp/rclcpp.hpp>
// #include <simple_action_server.hpp>
#include <turtle_bt_ros2/action/bt.hpp>
#include "rclcpp_action/rclcpp_action.hpp"
#include "behaviortree_cpp_v3/condition_node.h"

#include <tf2/exceptions.h>
#include <tf2_ros/transform_listener.h>
#include <tf2_ros/buffer.h>
#include <geometry_msgs/msg/twist.hpp>
# include <turtle_bt_ros2/srv/spawn.hpp>

#include <chrono>
#include <memory>
#include <string>

using std::placeholders::_1;
using namespace std::chrono_literals;
static int flag1 = 0;

class HaveEnemy : public BT::ConditionNode {
public:

    // using GoalHandle = rclcpp_action::ServerGoalHandle<turtle_bt_ros2::action::BT>;

    explicit HaveEnemy(const std::string &name, const BT::NodeConfiguration &config):
        BT::ConditionNode(name, config) {
        if (flag1 == 0)  {
            node_ = rclcpp::Node::make_shared(name);
            tf_buffer_have = std::make_unique<tf2_ros::Buffer>(node_->get_clock());
            transform_listener_have = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_have);
            flag1 = 1;
        }
        // this->actionserver_ = rclcpp_action::create_server<turtle_bt_ros2::action::BT>(
        //                                       node_->get_node_base_interface(),
        //                                       node_->get_node_clock_interface(),
        //                                       node_->get_node_logging_interface(),
        //                                       node_->get_node_waitables_interface(),
        //                                       "BTaction",
        //                                       std::bind(&HaveEnemy::execute_callback, this, _1));
    }

    HaveEnemy() = delete;

    ~HaveEnemy() {
        RCLCPP_DEBUG(node_->get_logger(), "Shutting down HaveEnemy BT node");
    }

    static BT::PortsList providedPorts() {
        return { BT::OutputPort < std::string > ("find  enemy") };
    }

    BT::NodeStatus tick() override {
        if (execute_callback()) {
            RCLCPP_INFO(node_->get_logger(), "Find enemy!");
            return BT::NodeStatus::SUCCESS;
        } else {
            RCLCPP_INFO(node_->get_logger(), "Everything is ok.");
            return BT::NodeStatus::FAILURE;
        }
    }
    bool execute_callback() {

        // node_->declare_parameter<std::string>("target_frame", "turtle1");
        // node_->get_parameter("target_frame", target_frame_);

        // tf_buffer_have = std::make_unique<tf2_ros::Buffer>(node_->get_clock());
        // transform_listener_have = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_have);

        RCLCPP_INFO(node_->get_logger(), "Starting Condition have_enemy");

        geometry_msgs::msg::TransformStamped transformStamped;

        rclcpp::WallRate loop_rate(5);
        loop_rate.sleep();

        transformStamped = tf_buffer_have->lookupTransform("turtle2", "turtle1", tf2::TimePointZero);

        distance_1 = sqrt(pow(transformStamped.transform.translation.x, 2) + pow(transformStamped.transform.translation.y, 2));

        if (distance_1 < 4.0) {
            return true;
        } else {
            return false;
        }
    }
private:
    rclcpp::Node::SharedPtr node_;
    // rclcpp_action::Server<turtle_bt_ros2::action::BT>::SharedPtr actionserver_;
    std::shared_ptr<tf2_ros::TransformListener> transform_listener_have{nullptr};
    std::unique_ptr<tf2_ros::Buffer> tf_buffer_have;
    // std::string target_frame_;
    double distance_1 ;
};